#include "mex.h"
#include "math.h"

//display Input array
//Muestra el vector de entrada
double * DisplayMatrix(char *Name, double *Data, int M, int N) { 
    int m, n;
    double *totalData;
    int nn;
    int tam;
    tam =(N*M);
    totalData = mxMalloc(tam*sizeof(double));
    
    for(n = 0; n < N; n++){
        for(m = 0; m < M; m++){
            nn=(m + M*n);
            //mexPrintf("%2.4f \n", Data[m + M*n] );
            totalData[nn] = Data[m + M*n];
        }
    }
    //totalData = mxRealloc(totalData, 15*sizeof(double));
    return totalData;
}
//Mesuare the euclidean distance of a choosen point and the others in 
// the input array
//Calcula la distancia a cada punto respecto al vector seleccionado.
double * euclideanDistance(double *Data, int M, int N, double *vector,double *euclideanDist){
    int m, n;
    double dist;
    double  *vectorDiffInd;
    vectorDiffInd = mxMalloc(M*sizeof(double));
    
    for(n =0; n < N; n++){
        dist = 0;
        for(m=0; m < M; m++){
            vectorDiffInd[m] = pow(vector[m]-Data[m + M*n], 2);
            dist +=vectorDiffInd[m];
            //mexPrintf("%4.2f \n", euclideanDist[n]);
        }
        euclideanDist[n] = sqrt(dist);
    }
    mxFree(vectorDiffInd);
    return euclideanDist;
}
//Find the index of each point that are not yet in the class, (clase)
int * find_class(bool *reach, int nReach, int N, int *clase, int group, int *indexReachPts){
    int n, nNewPts, iDiff;
    int *diff;
    nNewPts=0;
    nNewPts =get_count_value(reach, clase, group, N);
    
    diff = mxMalloc(nNewPts*sizeof(int));
    nNewPts=0;
    for(n = 0;n < N ; n++){
        if((reach[n] == true) && (!(clase[n]==group))){
            diff[nNewPts]=n;
            nNewPts++;
        }
    }
    iDiff=0;
    indexReachPts = mxRealloc(indexReachPts, (nReach + nNewPts)*sizeof(int));
    for(n = nReach;n < (nReach + nNewPts); n++){
        indexReachPts[n] = diff[iDiff];
        iDiff++;
    }
    mxFree(diff);
    return indexReachPts;
}
// Get how many points are not yet in the class, (clase)
int  get_count_value(bool *reach, int *clase, int group, int N){
    int nNewPts, n;
    nNewPts=0;
    for(n = 0;n < N ; n++){
        if((reach[n] == true) && (!(clase[n]==group))){
            nNewPts++;
        }
    }
    return nNewPts;
}
//reset reach array 
bool *reset_reach(bool *reach, int N){
    int n;
    for(n = 0;n < N; n++){
        reach[n]  = false;
    }
    return reach;
}
/* Data: Input Array
   M: rows
   N: Columns
   R: Ratio hipersperea
   NP: Number of point that must contain each cluster.
 */
int * DBSCAN(double *Data, int M, int N, double *R, double *NP){
    int m, n, counter, mm, nn, nPtos, group, i, nReach;
    double *euclideanDist, *point;
    bool *reach;
    int *clase, *indexReachPts;
    
    group = 0;
    nReach = 0;
    nPtos = 0;
    i=0;
    counter =0;
    // clase means class
    clase = mxCalloc(N, sizeof(int));
    reach = mxMalloc(N*sizeof(bool));
    point =mxMalloc(M*sizeof(double));
    indexReachPts =mxMalloc(nReach*sizeof(int));
    euclideanDist =mxMalloc(N*sizeof(double));

    /*
     * mexPrintf("%i \n", M);
     * mexPrintf("%i \n", N);
     * mexPrintf("%4.4f \n", R[0]);
     * mexPrintf("%4.4f \n", NP[0]);
     */
    
    for(n = N-1; n >= 0; n--) {
        //mexPrintf("%i \n", n);
        if(clase[n] == 0){
            for(m =0; m < M; m++){
                point[m]= Data[m + M*n];
                //mexPrintf("%8.4f \n", point[m]);
            }
            euclideanDist = euclideanDistance(Data, M, N, point,euclideanDist);
            
            nPtos=0;
            for(nn = 0; nn < N;nn++){
                if(euclideanDist[nn] < R[0]){
                    nPtos++;
                }
            }
            nPtos=nPtos-1;
            if((nPtos) < NP[0]){
                clase[n] = -1;
            }
            else{
                group++;
                reach = reset_reach(reach, N);
                indexReachPts = mxRealloc(indexReachPts,nReach*sizeof(int));
                for(nn = 0; nn < N;nn++){
                    if(euclideanDist[nn] < R[0]){
                        clase[nn] = group;
                        reach[nn] = true;
                        nReach++;
                        indexReachPts = mxRealloc(indexReachPts, (nReach)*sizeof(int));
                        indexReachPts[nReach-1] =nn;
                    }else{
                        reach[nn] = false;
                    }
                }             
                while ((nReach > 0)){
                    
                    i = indexReachPts[nReach-1];
                    for(mm = 0;mm < M; mm++ ){
                        point[mm]=Data[mm + M*i];
                    }
                    if(nReach > 1){
                        nReach--;
                        indexReachPts = mxRealloc(indexReachPts, (nReach)*sizeof(int));
                    }else{
                        nReach=0;
                    }
                    reach = reset_reach(reach, N);
                    euclideanDist = euclideanDistance(Data, M, N, point,euclideanDist);
                    
                    for(nn = 0; nn < N;nn++){
                        if(euclideanDist[nn] < R[0]){
                            reach[nn] = true;
                        }else{
                            reach[nn] = false;
                        }
                    }
                    counter=0;
                    indexReachPts = find_class(reach, nReach, N, clase, group, indexReachPts);
                    counter = get_count_value(reach, clase, group, N);
                    nReach = nReach + counter;
                    //mexPrintf("%2.4f \n", mxGetN(indexReachPts));
                    for(nn = 0; nn < N;nn++){
                        if(euclideanDist[nn] < R[0]){
                            clase[nn] = group;
                        }
                    } 
                }
            }
        }
    }
    mxFree(point);
    mxFree(indexReachPts);
    mxFree(euclideanDist);
    mxFree(reach);
    return clase;
}
//Mex Fuction
void mexFunction(int nlhs, mxArray *plhs[],
        int nrhs, const mxArray *prhs[]){
    double *F;
    int n;
    int *clase;
    int tamano;
    #define M_IN prhs[0]
            #define R_IN prhs[1]
            #define N_IN prhs[2]
            #define M_OUT plhs[0]
            tamano = (mxGetN(M_IN));
    M_OUT = mxCreateNumericMatrix(1, tamano, mxDOUBLE_CLASS, mxREAL);
    F=mxGetData(M_OUT);
    if (nrhs < 3)
        mexErrMsgTxt("Debe introducir la Matriz (3 X N), el radio de la hiperesfera y la cantidad minima de puntos");
    if (nlhs == 0)
        mexErrMsgTxt("Retorna un vector con las clases, debe especificar el vector que recibe");
    if (!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0]))
        mexErrMsgTxt("El parametro de entrada debe ser una Matrix tipo double");
    
    //F1=DisplayMatrix("Input", mxGetPr(M_IN), mxGetM(M_IN), mxGetN(M_IN));
    clase =DBSCAN(mxGetPr(M_IN), mxGetM(M_IN), mxGetN(M_IN), mxGetPr(R_IN), mxGetPr(N_IN));
    
    for(n = 0; n < (mxGetN(M_IN)); n++) {
        //mexPrintf("%2.4f ", F1[n]);
        //F[n]=F1[n];
        F[n]=clase[n];
        //mexPrintf("%2.4f ", F[n]);
        //mexPrintf("%i \n", clase[n]);
    }
    
    mxFree(clase);
    return;
}
